function waitI2C( port )
  while( 0 ~= nxt.I2CGetStatus( port ) ) do
  end
end

function setupI2C(port)
  nxt.InputSetType(port,2)
  nxt.InputSetDir(port,1,1)
  nxt.InputSetState(port,1,1)

  nxt.I2CInitPins(port)
end

--[[
Codes for IRHeading function

42H - DC Direction (0 - 9)

49H - AC Direction Data (0 - 9)
]]


function IRHeading(port)
	nxt.I2CSendData(port, string.char( 0x10, 0x49 ), 8)
	waitI2C(port)
	return string.byte(nxt.I2CRecvData(port))
end

function checkIR(port)
    while nxt.ButtonRead() == 0 do
        print(IRHeading(port))
    end
end

function goRight(mLeft, mRight, power)
    nxt.OutputSetSpeed(mLeft, 32, power)
    nxt.OutputSetSpeed(mRight, 32, -power)
end

function goLeft(mLeft, mRight, power)
    nxt.OutputSetSpeed(mLeft, 32, -power)
    nxt.OutputSetSpeed(mRight, 32, power)
end

function goForward(mLeft, mRight, power)
    nxt.OutputSetSpeed(mLeft, 32, power)
    nxt.OutputSetSpeed(mRight, 32, power)
end

function cutMotors(mLeft, mRight)
    nxt.OutputSetSpeed(mLeft, 32, 0)
    nxt.OutputSetSpeed(mRight, 32, 0)
end

function pauseSys(time)
    a = nxt.TimerRead()
    while nxt.TimerRead() < a+time do
    end
end


function navigateToBall(port, mLeft, mRight, power)
    
    setupI2C(port)
    
    while nxt.ButtonRead() == 0 do
        --print(IRHeading(port))
        heading = IRHeading(port)
        print('Heading:' + heading)
        
        if heading == 0 then
            cutMotors(mLeft, mRight)
        end
            
        if heading == 5 then
            --goForward(mLeft, mRight, power)
            cutMotors(mLeft, mRight, power)
        end
        
        if heading < 5 then
            goLeft(mLeft, mRight, power)
        else if heading > 5 then
            goRight(mLeft, mRight, power)
        end
                    

            
        checkIR(port)
        pauseSys(200)
        
    end
    cutMotors(mLeft, mRight)
end






